#pragma once
#define WITH_CAL
#include <stdarg.h>
#include <stdio.h>
//#include <slave.h>
typedef struct cal_lock{
  int req, cur;
} cal_lock_t;
__attribute__((weak)) cal_lock_t __cal_global_lock = {0, 0};

static void cal_lock(cal_lock_t *lock){
  int req, cur;
  long ptr;
  asm ("ldi  %3, %2\n\t"
       "faaw %0, 0(%3)\n\t"
       "1:\n\t"
       "ldw  %1, 4(%3)\n\t"
       "subw %1, %0, %1\n\t"
       "bne  %1, 1b\n\t"
       : "=r"(req), "=r"(cur), "+m"(*lock), "=r"(ptr));
}

static void cal_unlock(cal_lock_t *lock){
  long ptr;
  asm ("ldi  %1, %0\n\t"
       "faaw $31, 4(%1)\n\t" :"+m"(*lock), "=r"(ptr));
}
static void cal_global_lock(){
  cal_lock(&__cal_global_lock);
}

static void cal_global_unlock(){
  cal_unlock(&__cal_global_lock);
}

static void cal_locked_printf(const char *fmt, ...){
  volatile long vsprintf_addr = (long)vsprintf;
  volatile long printf_addr = (long)printf;
  int (*vsprintf_ptr)(char *, const char *, va_list) = (int(*)(char *, const char *, va_list))vsprintf_addr;
  char buf[256];
  va_list va;
  va_start(va, fmt);
  vsprintf_ptr(buf, fmt, va);
  va_end(va);
  cal_global_lock();
  fputs(buf, stdout);
  fflush(stdout);
  cal_global_unlock();
}
